
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
pub enum CellType {
    // 可行走区域
    FREE = 0,
    // 未知区域
    UNKNOWN = 125,
    // 不可行走的区域，机器人参考中心落在点有关
    INSCRIBED_OBSTRACLE = 250,
    // 静态障碍物区域
    OBSTRACLE = 255,

}

impl CellType {
    pub fn get_value(&self) -> u8 {
        *self as u8
    }
    pub fn u8_enum(val: u8) -> CellType{

        if val < 10 {
            CellType::FREE
        }else if val < 250 {
            CellType::UNKNOWN
        }else if val < 255 {
            CellType::INSCRIBED_OBSTRACLE
        }else {
            CellType::OBSTRACLE
        }

    }

    pub fn enum_u8(&self) -> u8 {
        *self as u8
    }
}
#[derive(Clone, Debug, PartialEq)]
pub struct GridMap{
    width: u32,
    height: u32,
    resolution: f32,
    // 采用一维数组存储二维表格
    cells: Vec<CellType>,
}

impl GridMap {
    pub fn new(width: u32, height: u32, resolution: f32) -> Self {
        GridMap {
            width: width,
            height: height,
            resolution: resolution,
            cells: vec![CellType::FREE; (width * height) as usize]
        }
    }

    pub fn get_width(&self) -> u32 {
        self.width
    }

    pub fn get_height(&self) -> u32 {
        self.height
    }

    pub fn get_resolution(&self) -> f32 {
        self.resolution
    }
    // 获取单元格列表
    pub fn get_cells(&self) -> &Vec<CellType> {
        &self.cells
    }

    pub fn set_cell(&mut self, x: u32, y: u32, cell_type: CellType) {
        // image coord:left-top 
        // |-----> width(x)
        // |
        // |height(y)
        // self.cells[self.get_xy2idx(x, y)] = cell_type; ???
        self.cells[(y * self.width + x) as usize] = cell_type;
    }
    pub fn set_cell_u8(&mut self, x: u32, y: u32, val: u8) {
        // image coord:left-top 
        // |-----> width(x)
        // |
        // |height(y)
        // self.cells[self.get_xy2idx(x, y)] = cell_type; ???
        self.cells[(y * self.width + x) as usize] = CellType::u8_enum(val);
    }

    pub fn get_cell(&self, x: u32, y: u32) -> CellType {
        self.cells[self.get_xy2idx(x, y)]
    }

    pub fn get_xy2idx(&self, x: u32, y:u32) -> usize {
    // 将像素索引坐标转换为Vec 索引
        (y * self.width + x) as usize
    }

    pub fn get_grididx_xy(&self, x: f32, y: f32) -> (u32, u32) {
        // 将地图坐标(单位米)转换为栅格地图索引
        ((x / self.resolution) as u32, (y / self.resolution) as u32)
    }

    pub fn is_travelable(&self, x: u32, y: u32) -> bool {
        match self.get_cell(x, y) {
            CellType::FREE => true,
            CellType::UNKNOWN => true,
            _ => false
        }
    }

}

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn test_gridmap_attributes() {
        let gridmap = GridMap::new(10, 10, 1.0);
        assert_eq!(gridmap.get_width(), 10);
        assert_eq!(gridmap.get_height(), 10);
        assert_eq!(gridmap.get_resolution(), 1.0);
    }
    #[test]
    fn test_gridmap_insert() {

        let mut gridmap = GridMap::new(10, 10, 1.0);
        gridmap.set_cell(0, 0, CellType::OBSTRACLE);
        assert_eq!(gridmap.get_cell(0, 0), CellType::OBSTRACLE);
        gridmap.set_cell(1, 0, CellType::FREE);
        assert_eq!(gridmap.get_cell(1, 0), CellType::FREE);
        gridmap.set_cell(2, 0, CellType::INSCRIBED_OBSTRACLE);
        assert_eq!(gridmap.get_cell(2, 0), CellType::INSCRIBED_OBSTRACLE);
        gridmap.set_cell(5, 0, CellType::OBSTRACLE);
        assert_eq!(gridmap.get_cell(5, 0), CellType::OBSTRACLE);
        gridmap.set_cell(3, 4, CellType::OBSTRACLE);
        assert_eq!(gridmap.get_cell(3, 4), CellType::OBSTRACLE);
       
       gridmap.set_cell_u8(3, 5, 125);
       assert_eq!(gridmap.get_cell(3, 5), CellType::FREE);

    }
    #[test]
    fn test_cell_xy() {

        let mut gridmap = GridMap::new(10, 10, 1.0);
        gridmap.set_cell(5, 6, CellType::OBSTRACLE);
        assert_eq!(gridmap.get_cell(5, 6), CellType::OBSTRACLE);
        gridmap.set_cell(5, 7, CellType::FREE);
        assert_eq!(gridmap.get_cell(5, 7), CellType::FREE);
        gridmap.set_cell(5, 8, CellType::UNKNOWN);
        assert_eq!(gridmap.get_cell(5, 8), CellType::UNKNOWN);

        assert_eq!(gridmap.is_travelable(5, 6), false);
        assert_eq!(gridmap.is_travelable(5, 7), true);
        assert_eq!(gridmap.is_travelable(5, 8), true);



    }

    #[test]
    fn test_enum_val() {
        let val = CellType::u8_enum(125);
    
        assert_eq!(val.enum_u8(), 125);
    }

}
